#include "main_while.hpp"
#include <chrono>
#include <memory>
#include <iostream>
Wheel_in_out_module::Wheel_in_out_module()
{
  w_msg_in.resize(wheel_msg_in_len);
  for(auto& i:w_msg_in) i=0;
  w_msg_out.resize(wheel_msg_out_len);
  for(auto& i:w_msg_out) i=0;
    bm=new bianma("ttyS7",1000000);
    yg=new yaogan("ttyS6",115200);
    wlc=new wheelctr("ttyS4",1000000);
    w_module1=new wheel_module();
    w_module2=new wheel_module();
    w_module3=new wheel_module();
    w_module4=new wheel_module();
    dc = new Data_Class();
    //for(int i=0;i<sizeof(msg_set);i++){ msg_set.msg[i]=0;msg_cur.msg[i]=0;}
    test_flag=1;
    work_mode=1;
}
  void Wheel_in_out_module::node_topic_callback( std::vector<std::int32_t> & msg)
  {
    dc->data_msg_whell_control(msg);
   
   }
  void Wheel_in_out_module::node_time_callback( std::vector<std::int32_t> & msg)
  {
    Data_swap();
    
    dc->data_info_wheel_msg(msg);
    std::cout << "curangle ";
    for(int i=0;i<4;i++) std::cout<<i<<"="<<bm->current_angle[i]<<" ";
    std::cout<<std::endl;
    
  }
  ////手柄直接遥控
   void Wheel_in_out_module::Run_yaokong(void)
   {

   }
  void Wheel_in_out_module::Data_swap(void)
  {
      w_module1->set_module_speed(dc->speedset[0]);
      w_module2->set_module_speed(dc->speedset[1]);
      w_module3->set_module_speed(dc->speedset[2]);
      w_module4->set_module_speed(dc->speedset[3]);

      w_module1->set_module_angle(dc->angleset[0]);
      w_module2->set_module_angle(dc->angleset[1]);
      w_module3->set_module_angle(dc->angleset[2]);
      w_module4->set_module_angle(dc->angleset[3]);

      dc->anglecur[0] = bm->current_angle[0];
      dc->anglecur[1] = bm->current_angle[1];
      dc->anglecur[2] = bm->current_angle[2];
      dc->anglecur[3] = bm->current_angle[3];
      for(int i=0;i<8;i++)
        dc->speedcur[i] = wlc->current_speed[i];
      
    
       dc->yaogan[0]=yg->yg_value.Left_horiz; dc->yaogan[1]=yg->yg_value.Left_vertic;
        dc->yaogan[2]=-yg->yg_value.Right_horiz; //左右换向
        dc->yaogan[3]=yg->yg_value.Right_vertic;
        dc->otherout[1]=yg->yg_value.key1;dc->otherout[2]=yg->yg_value.key2;
     

      w_module1->set_angle_cur(bm->current_angle[0]);
      w_module1->conclude();
      wlc->set_speed[0]=w_module1->speed_set_L;
      wlc->set_speed[1]=w_module1->speed_set_R;

      w_module2->set_angle_cur(bm->current_angle[1]);
      w_module2->conclude();
      wlc->set_speed[2]=w_module2->speed_set_L;
      wlc->set_speed[3]=w_module2->speed_set_R;

      w_module3->set_angle_cur(bm->current_angle[2]);
      w_module3->conclude();
      wlc->set_speed[4]=w_module3->speed_set_L;
      wlc->set_speed[5]=w_module3->speed_set_R;

      w_module4->set_angle_cur(bm->current_angle[3]);
      w_module4->conclude();
      wlc->set_speed[6]=w_module4->speed_set_L;
      wlc->set_speed[7]=w_module4->speed_set_R;
  }
//void Wheel_in_out_module::wheel_module_out(){}
